#include "DoorDetector.hh"
#include "IntervalSplitter.h"
#include "EndPointSplitter.h"
#include "CompositeSplitter.h"
#include "ConfigFile.h"
#include "la2d.h"
#include <algorithm>   // for swap()
#include <cmath>       // for fabs()

DoorDetector::DoorDetector(std::string configFilename) : splitter_(0)
{
  util::ConfigFile conf(configFilename);

  // smoothVel()'s parameters
  DOOR_WIDTH_MEAN = conf.getDouble("DOOR_WIDTH_MEAN", 0.90);
  DOOR_WIDTH_TOLL = conf.getDouble("DOOR_WIDTH_TOLL", 0.10);
  DOOR_ALLIGN_THR = conf.getDouble("DOOR_ALLIGN_THR", 0.05);
  DOOR_PILLAR_MIN_SIZE = conf.getDouble("DOOR_PILLAR_MIN_SIZE", 0.10);
  DOOR_INT_SPLITTER_BASE = conf.getDouble("DOOR_INT_SPLITTER_BASE", 0.1);
  DOOR_INT_SPLITTER_STEP = conf.getDouble("DOOR_INT_SPLITTER_STEP", 0.1);
  DOOR_END_SPLITTER_TH = conf.getDouble("DOOR_END_SPLITTER_TH", 0.1);
  DOOR_END_SPLITTER_LEVEL = conf.getDouble("DOOR_END_SPLITTER_LEVEL", 0.03);
  DOOR_END_SPLITTER_POINTNUM = conf.getInt("DOOR_END_SPLITTER_POINTNUM", 5);

  splitter_ = new line::CompositeSplitter(
                new line::IntervalSplitter(DOOR_INT_SPLITTER_BASE,
                                           DOOR_INT_SPLITTER_STEP),
                new line::EndPointSplitter(DOOR_END_SPLITTER_TH,
                                           DOOR_END_SPLITTER_LEVEL,
                                           DOOR_END_SPLITTER_POINTNUM));
}

DoorDetector::~DoorDetector()
{
  delete splitter_;
}

void DoorDetector::findDoors(const line::point_vector& scan,door_vector& doors)
{
  line::interval_vector iv;
  door_type door;
  double pillarAlign;
  double doorWidth;

  // Computes the pairs of points that are endpoints of segments.
  splitter_->split(scan,iv);

  // Doors are detected by two segments (i.e. two iv[i] and iv[j]) 
  // that lie on the same line (with some tollerance) and the following 
  // conditions hold:
  // 1) the two segments lie on the same line
  // 2) the gap between the two segments is about the width of the door
  // 3) the lengths of the two segments is above a threshold 
  //    DOOR_PILLAR_MIN_SIZE
  for (unsigned int i = 0; i < iv.size(); ++i) {
    line::point_type pmax = scan[iv[i].first];
    line::point_type pmin = scan[iv[i].second];
    if (distance(pmin,pmax) < DOOR_PILLAR_MIN_SIZE) continue;
    pillarAlign = 1e+6;
    doorWidth = 1e+6;
    for (unsigned int j = i+1; j < iv.size(); ++j) {
      line::point_type qmin = scan[iv[j].first];
      line::point_type qmax = scan[iv[j].second];
      if (distance(qmin,qmax) < DOOR_PILLAR_MIN_SIZE) continue;
      swapPoint(pmin,pmax,qmin,qmax);
      pillarAlign = alignment(pmin,pmax,qmin,qmax); 
      doorWidth = gapSize(pmin,pmax,qmin,qmax);
      if (pillarAlign < 2.0 * DOOR_ALLIGN_THR && 
          fabs(doorWidth-DOOR_WIDTH_MEAN) < DOOR_WIDTH_TOLL) 
      {
        door.center.x = 0.5 * (pmin.x + qmin.x);
        door.center.y = 0.5 * (pmin.y + qmin.y);
        door.first = pmin;
        door.second = qmin;
        door.width = doorWidth;
        door.thetaNorm = atan2(-(qmin.x-pmin.x), (qmin.y,pmin.y));
        doors.push_back(door); 
  break;
      }
      
      // When a segment j-th aligned with segment i-th is found, we stop 
      // the search. 
      // Two pillars (i.e. two aligned segments verifying the above conditions) 
      // cannot have an aligned segment in the middle!!
      if (pillarAlign < 2.0 * DOOR_ALLIGN_THR) {
  //break;
      }
    }
  }
  
  //plotGnuplot(scan,iv,doors);
}

double DoorDetector::alignment(line::point_type pmin,line::point_type pmax,
                               line::point_type qmin,line::point_type qmax)
{
  double plen = distance(pmin,pmax);
  double qlen = distance(qmin,qmax);
  double dmax = distance(pmax,qmax);
  double dp = distance(pmin,qmax);
  double dq = distance(pmax,qmin);
  return ((plen + dp - dmax) + (qlen + dq - dmax)); 
}

// Returns the size of the gap between the two segments p and q.
// Gap size is estimated as the distance between the two closest 
// points of p and q.
// (Pre-condition: check that the two segments lie on the same line!)
double DoorDetector::gapSize(line::point_type pmin,line::point_type pmax,
                             line::point_type qmin,line::point_type qmax)
{
  return distance(pmin,qmin);
}

// Swaps the points so that 
// a) p1 and q1 are the closest points of segments p and q 
// b) p2 and q2 are the farthest points of segments p and q
void DoorDetector::swapPoint(line::point_type& p1,line::point_type& p2,
                             line::point_type& q1,line::point_type& q2)
{
  double d11 = distance(p1,q1);
  double d12 = distance(p1,q2);
  double d21 = distance(p2,q1);
  double d22 = distance(p2,q2);
  if (d12 < d11 && d12 < d21 && d12 < d22) {
    std::swap(q1,q2);
  }
  else if (d21 < d11 && d21 < d12 && d21 < d22) {
    std::swap(p1,p2);
  }
  else if (d22 < d11 && d22 < d12 && d22 < d21) {
    std::swap(p1,p2);
    std::swap(q1,q2);
  } 
}

void DoorDetector::plotGnuplot(const line::point_vector& scan,line::interval_vector& iv,door_vector& doors)
{
  if (iv.empty() && doors.empty()) return;
  std::cout << "set size ratio -1" << std::endl;
  std::cout << "plot '-' w l";
  if (!doors.empty()) {
    std::cout << ", '-'";
  }
  std::cout << std::endl;
  for (unsigned int i = 0; i < iv.size(); ++i) {
    std::cout << scan[iv[i].first].x << " " << scan[iv[i].first].y << std::endl; 
    std::cout << scan[iv[i].second].x << " " << scan[iv[i].second].y << std::endl;
    std::cout << std::endl;
  }
  std::cout << "e" << std::endl;
  if (!doors.empty()) {
    for (unsigned int i = 0; i < doors.size(); ++i) {
    if(!((doors[i].center.x<0.4 && doors[i].center.y<0.4) || (doors[i].center.x>2.4 && doors[i].center.y>2.4)))
      std::cout << doors[i].center.x << " " << doors[i].center.y << std::endl; 
    }
    std::cout << "e" << std::endl;
  }
}

double DoorDetector::distance(line::point_type p1,line::point_type p2)
{
  return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y));
}
